#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Description:       :
@Date     :2021/04/01 10:08:53
@Author      :chenqi
@version      :1.0
'''

# from Cloud_Merge import VehicleMessage
# from Cloud_Merge.Cloud_Merge import Obstacle
from typing import List
from utils.latlontomap import rad
import numpy as np
from math import sin, cos


def rotatation_matrix(heading: float) -> np.array:
    """
    根据车辆heading获取旋转矩阵

    Args:
        heading (float): 车辆朝向，angle转radian
        [
            cos(Θ)， -sin(Θ)
            sin(Θ), cos(Θ)
        ]

    Returns:
        np.array: 旋转矩阵
    """
    theta = rad(heading)
    return np.array([[cos(theta), -sin(theta)],
                     [sin(theta), cos(theta)]])


def transform_martix(x: float, y: float) -> np.array:
    """
    根据车辆坐标(x, y)获取平移矩阵

    Args:
        x (float): x
        y (float): y

    Returns:
        np.array: 平移矩阵
    """
    return np.array([x, y])


def distance2xy(azimuth: float, distance: float):
    """根据距离和旋转角计算真实世界中的x,y值

    Args:
        azimuth (float): 旋转角
        distance (float): 与原点距离

    Returns:
        tuple(x, y): x,y组成的元组
    """
    theta = rad(azimuth)
    return (cos(theta) * distance, sin(theta) * distance)

# def jsonParser(root:str) -> VehicleMessage:
#     pass


def pretty_print(clas, indent=0):
    print(' ' * indent + type(clas).__name__ + ':')
    indent += 4
    for k, v in clas.__dict__.items():
        if '__dict__' in dir(v):
            pretty_print(v, indent)
        else:
            print(' ' * indent + k + ': ' + str(v))


def list_obstacle(obs) -> np.array:
    l = len(obs)
    arr = np.arange(l*11, dtype=float).reshape(l, 11)
    print(arr.shape)
    i = 0
    for o in obs:
        arr[i][0] = 0
        arr[i][1] = o.classification
        arr[i][2] = o.score
        arr[i][3] = o.center_x
        arr[i][4] = o.center_y
        arr[i][5] = o.height
        arr[i][6] = o.length
        arr[i][7] = o.width
        arr[i][8] = 0
        arr[i][9] = 0
        arr[i][10] = o.heading
        i += 1
    return arr


def save_csv(obs, file_name='merged.csv'):
    import pandas
    arr = list_obstacle(obs)
    dataframe = pandas.DataFrame(arr, columns=["counter_time", "classificaton", "score", "center_x",
                                               "center_y", "height", "length", "width", "obj_longitude",
                                               "obj_latitude", "heading"])
    dataframe.to_csv(file_name)
